//
//  KinematicSim.cpp
//  TrackingSim
//
//  Created by Garrett Manka on 11/25/11.
//  Copyright (c) 2011. All rights reserved.
//

#include <sim/KinematicSim.h>
#include <sim/Scenario.h>
#include <sim/Clock.h>

#include <iostream>

namespace Sim
{
    KinematicSim* KinematicSim::s_the_instance = NULL;
    bool KinematicSim::s_instance_created = false;
    
    KinematicSim::KinematicSim():m_clock(new Utilities::Clock())
    {
    }
    
    KinematicSim::KinematicSim(Scenario* scenario):m_clock(new Utilities::Clock())
    {
        //Copy the passed in scenario into the kinematic sim object
        m_scenario = new Scenario(scenario->getUpdateFrequency());
        *m_scenario = *scenario;
    } 
    
    KinematicSim::~KinematicSim()
    {
        delete m_clock;
        delete s_the_instance;
        delete m_scenario;
        s_the_instance = NULL;
        s_instance_created = false;
    }
    
    void KinematicSim::create(Sim::Scenario* scenario)
    {
        if(!s_instance_created)
        {
            s_the_instance = new KinematicSim(scenario);
            s_instance_created = true;
        }
    }
    
    KinematicSim* KinematicSim::getKS()
    {
        if(!s_instance_created)
        {
            s_the_instance = new KinematicSim();
            s_instance_created = true;
        }
        
        return s_the_instance;
    }
    
    Scenario* KinematicSim::getScenario() const
    {
        return m_scenario;
    }
    
    Utilities::Clock* KinematicSim::getClock() const
    {
        return m_clock;
    }
    
    void KinematicSim::update()
    {
        std::list< Player* >::iterator list_iter;
        
        for( list_iter = m_scenario->getPlayerList().begin() ; list_iter != m_scenario->getPlayerList().end() ; list_iter++ )
        {
            Player* player = *list_iter;
            
            Vector accel = player->getAcceleration();
            Vector vel = player->getVelocity();
            Vector pos = player->getPosition();
  
            double t = m_scenario->getUpdatePeriod();
            
            pos = pos + 0.5 * accel * t * t + vel * t;
            
            vel = vel + accel * t;
            
            player->setPosition(pos);
            player->setVelocity(vel);
        }
        
        m_clock->markCurrentTime();
    }
    
    void KinematicSim::update(double dt)
    {
        std::list< Player* >::iterator list_iter;
        
        for( list_iter = m_scenario->getPlayerList().begin() ; list_iter != m_scenario->getPlayerList().end() ; list_iter++ )
        {
            Player* player = *list_iter;
            
            Vector accel = player->getAcceleration();
            Vector vel = player->getVelocity();
            Vector pos = player->getPosition();
        
            pos = pos + 0.5 * accel * dt * dt + vel * dt;
            
            vel = vel + accel * dt;
            
            player->setPosition(pos);
            player->setVelocity(vel);
        }
    }
    
    bool KinematicSim::updateNeeded() const
    {
        double elapsed_seconds = m_clock->getTimeInSecondsFromMark();
        
        if( elapsed_seconds > m_scenario->getUpdatePeriod() )
        {
            return true;
        }
        else
        {
            return false;
        }
    }
}
